import machine 
import time
import math

#PWM引脚定义
PWM1 = machine.PWM(machine.Pin(13))
PWM2 = machine.PWM(machine.Pin(12))
PWM3 = machine.PWM(machine.Pin(14))
PWM4 = machine.PWM(machine.Pin(27))

PWM1.duty_u16(0)
PWM2.duty_u16(65535)
PWM3.duty_u16(0)
PWM4.duty_u16(65535)

###### 定义引脚
#ADC引脚定义
INZ = machine.Pin(15)
INX = machine.Pin(2)
INY = machine.Pin(4)

adcZ = machine.ADC(INZ)
adcX = machine.ADC(INX)
adcY = machine.ADC(INY)

adcX.atten(machine.ADC.ATTN_11DB)  # 电压范围 0-3.3V
adcY.atten(machine.ADC.ATTN_11DB)  # 电压范围 0-3.3V
adcZ.atten(machine.ADC.ATTN_11DB)  # 电压范围 0-3.3V

# 初始化一个空列表用于存储历史值
history_Z = []
history_X = []
history_Y = []
history_length = 5  # 定义滤波的历史值个数

while True:
    # 读取ADC值
    valZ = adcZ.read_u16()  # 读取原始模拟值，范围为 0-65535
    valX = adcX.read_u16()
    valY = adcY.read_u16()
    
    # 将 ADC 值转换为电压（假设 Vref 为 3.3V）
    voltageZ = valZ * (3.3 / 65535)
    voltageX = valX * (3.3 / 65535)
    voltageY = valY * (3.3 / 65535)

    # 添加新的值到历史列表并限制长度
    history_Z.append(voltageZ)
    history_X.append(voltageX)
    history_Y.append(voltageY)
    
    if len(history_Z) > history_length:
        history_Z.pop(0)  # 移除最旧的值
    if len(history_X) > history_length:
        history_X.pop(0)
    if len(history_Y) > history_length:
        history_Y.pop(0)

    # 计算滤波后的电压平均值
    avg_voltageZ = sum(history_Z) / len(history_Z)
    avg_voltageX = sum(history_X) / len(history_X)
    avg_voltageY = sum(history_Y) / len(history_Y)

    # 打印输出当前值和滤波后的电压
    print("ADC Value:", valZ, "   |   ", 
          "Avg VoltageZ:", f"{avg_voltageZ:.2f}", 
          " Avg VoltageX:", f"{avg_voltageX:.2f}", 
          " Avg VoltageY:", f"{avg_voltageY:.2f}")

    time.sleep(0.3)  # 每0.3秒读取一次

    

# PID参数
Setpoint_X = 500
Input_X = 0
Output_X = 0
X_plus = 0

Setpoint_Y = 500
Input_Y = 0
Output_Y = 0
Y_plus = 0

p_X, i_X, d_X = 1, 0, 0.008
p_Y, i_Y, d_Y = 1, 0, 0.008

run_flag = 0
run_dirict = 0
go_step = 0
time_last = 0

# PID控制器类
class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0
        self.integral = 0

    def compute(self, input_value, setpoint):
        error = setpoint - input_value
        self.integral += error
        derivative = error - self.last_error
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.last_error = error
        return output

pid_x = PID(p_X, i_X, d_X)
pid_y = PID(p_Y, i_Y, d_Y)

# 控制电机的函数
def turn_X(a):
    if a >= 0:
        IN1.on()
        IN2.off()
        ENA.duty(a)
    else:
        IN1.off()
        IN2.on()
        ENA.duty(-a)

def turn_Y(a):
    if a >= 0:
        IN3.off()
        IN4.on()
        ENB.duty(a)
    else:
        IN3.on()
        IN4.off()
        ENB.duty(-a)

def setup():
    ENA.duty(0)
    ENB.duty(0)
    BL.off()
    print("Setup completed.")

def loop():
    global Input_X, Input_Y, go_step, time_last, run_flag, run_dirict

    while True:
        print("Setpoint_X:", Setpoint_X)
        print("Setpoint_Y:", Setpoint_Y)

        # 限制Setpoint范围
        Setpoint_X = max(480, min(Setpoint_X, 575))
        Setpoint_Y = max(480, min(Setpoint_Y, 575))

        Input_X = machine.ADC(machine.Pin(2)).read()  # 假设 A1 连接到 34 号引脚
        Input_Y = machine.ADC(machine.Pin(4)).read()  # 假设 A0 连接到 35 号引脚

        if machine.ADC(machine.Pin(15)).read() > 450:  # 假设 A2 连接到 32 号引脚
            BL.on()
            on_put = 1
        else:
            BL.off()
            on_put = 0

        if on_put:
            Output_X = pid_x.compute(Input_X, Setpoint_X)
            Output_Y = pid_y.compute(Input_Y, Setpoint_Y)
            turn_X(Output_X + X_plus)
            turn_Y(Output_Y + Y_plus)

            if run_flag:
                if time.ticks_ms() - time_last > 2:
                    time_last = time.ticks_ms()
                    if run_dirict:
                        X_plus = 25 * math.cos(go_step)
                        Y_plus = 25 * math.sin(go_step)
                    else:
                        X_plus = 25 * math.sin(go_step)
                        Y_plus = 25 * math.cos(go_step)
                    go_step += 0.07
                    if go_step > 6.3:
                        go_step = 0
        else:
            turn_X(0)
            turn_Y(0)

        print("Input_X:", Input_X)
        print("Input_Y:", Input_Y)

# 初始化并进入主循环
print("Starting...")
setup()
loop()


